#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>

#include <opencv2/opencv.hpp>
#include "mv_pub.hpp"

using namespace std;

//#define HGUI_DEBUG

void MvPub::drawXLine(Mat& src, Point2f &pt, Scalar sclr, int width, int len) {
	Point up, down, left, right;

	up = down = left = right = pt;

	up.y = up.y - len;
	down.y = down.y + len;
	left.x = left.x - len;
	right.x = right.x + len;

	line(src, up, down, sclr, width);
	line(src, left, right, sclr, width);
}

// 生成7x7十字模板(粗线)
void MvPub::makeTemp7x7Strong(OutputArray temp) {
	Mat m(7, 7, CV_8UC1, Scalar(0));

	m.col(2).setTo(Scalar(1));
	m.col(3).setTo(Scalar(1));
	m.col(4).setTo(Scalar(1));

	m.row(2).setTo(Scalar(1));
	m.row(3).setTo(Scalar(1));
	m.row(4).setTo(Scalar(1));

	m.copyTo(temp);
}

// 生成7x7粗反向十字模板
void MvPub::makeTemp7x7StrongInv(OutputArray temp) {
	Mat m(7, 7, CV_8UC1, Scalar(1));

	m.col(2).setTo(Scalar(0));
	m.col(3).setTo(Scalar(0));
	m.col(4).setTo(Scalar(0));

	m.row(2).setTo(Scalar(0));
	m.row(3).setTo(Scalar(0));
	m.row(4).setTo(Scalar(0));

	m.copyTo(temp);
}

Mat MvPub::makeTempX(int x, int y) {

	if (x == 0 || y == 0) {
		x = 3;
		y = 3;
	}

	Mat tempx(y, x, CV_8UC1, Scalar(255));

	tempx.col(x / 2).setTo(Scalar(0));
	tempx.row(y / 2).setTo(Scalar(0));

	cout << tempx << endl;

	return tempx;
}

Mat MvPub::makeInvTempX(int x, int y) {

	if (x == 0 || y == 0) {
		x = 3;
		y = 3;
	}

	Mat tempx(y, x, CV_8UC1, Scalar(0));

	tempx.col(x / 2).setTo(Scalar(255));
	tempx.row(y / 2).setTo(Scalar(255));

	cout << tempx << endl;

	return tempx;
}

// 查找十字交叉点
// src:源图像
// pts:输出座标
int MvPub::findCrossPoints(Mat &src, OutputArray o) {

	Mat bak;
	src.copyTo(bak);

	adaptiveThreshold(src, src, 255, CV_ADAPTIVE_THRESH_MEAN_C, THRESH_BINARY, 63, 7);
//	threshold(src, src, 170, 255, THRESH_BINARY);

	Mat ele0 = getStructuringElement(MORPH_RECT, Size(3, 3));
	erode(src, src, ele0);
	Mat ele1 = getStructuringElement(MORPH_RECT, Size(3, 3));
	dilate(src, src, ele1);

	Mat temp = makeInvTempX(21, 29);

#ifdef HGUI_DEBUG
	namedWindow("fcp1");
	imshow("fcp1", temp);

	namedWindow("fcp0");
	imshow("fcp0", src);
	waitKey(0);
#endif

	filter2D(src, src, 8, temp);

#ifdef HGUI_DEBUG
	namedWindow("fcp0", 0);
	imshow("fcp0", src);
	waitKey(0);
#endif
	vector < vector<Point> > contours;
	vector<Vec4i> hierarchy;
	findContours(src, contours, hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);

	// 计算轮廓矩
	vector<Moments> mu(contours.size());
	for (unsigned int i = 0; i < contours.size(); i++) {
		mu[i] = moments(contours[i], false);
	}

	//计算轮廓的质心,过虑不需要点
	cvtColor(src, src, CV_GRAY2RGB);
	cvtColor(bak, bak, CV_GRAY2RGB);

	int cnt = 0;
	vector<Point2f> vo;
	vector<Point2f> mc(contours.size());
	for (unsigned int i = 0; i < contours.size(); i++) {

		mc[i] = Point2d(mu[i].m10 / mu[i].m00, mu[i].m01 / mu[i].m00);
		if (mu[i].m00 > 2 && mu[i].m00 < 400) {
			// 只有符合条件的才打印
			if (mc[i].y < 288 + 8 && mc[i].y > 288 - 8) {
				drawXLine(src, mc[i], CV_RGB(0, 255, 0), 1, 15);
				circle(src, mc[i], 4, CV_RGB(0, 255, 0), 1);

				vo.push_back(mc[i]);

				Point dispPoint;
				dispPoint.x = mc[i].x;
				dispPoint.y = mc[i].y;
				dispPoint.y -= ((cnt++) % 3) * 10;

				char tmp[64];
				sprintf(tmp, "%0.2f,%0.2f", mc[i].x, mc[i].y);
				putText(bak, tmp, dispPoint, 1, 1, Scalar(0, 0, 128));
			}
		}
	}

#ifdef HGUI_DEBUG
	imshow("fcp0", bak);
	waitKey(0);
#endif

	Mat mm(vo);
	mm.copyTo(o);
	cout << o.getMat() << endl;

	return 0;
}

// 球在映射
// x0,y0:圆心座标；u,v:源座标
float MvPub::getCircleMapSrcX(int u, int v, float x0, float y0, float r) {

	float xh = u - x0;
	float yi = y0 - v;

	float dx = sqrt(r * r - yi * yi);
	float xk = (x0 + xh * dx / r);

	return xk;
}
float MvPub::getCircleMapSrcY(int u, int v, float x0, float y0, float r) {

	float yh = v - y0;
	float xi = x0 - u;

	float dy = sqrt(r * r - xi * xi);
	float yk = (y0 + yh * dy / r);

	return yk;
}
Point2f MvPub::getCircleMapSrcPoint(int u, int v, float x0, float y0, float r) {

	Point2f p(0.f, 0.f);

	p.x = getCircleMapSrcX(u, v, x0, y0, r);
	p.y = getCircleMapSrcY(u, v, x0, y0, r * 10 / 12);

	return p;
}

void MvPub::getCircleMapSrcPic(Mat &dst, Mat &src, int r) {

	Point2f xy(0.f, 0.f);
	xy.x = src.size().width / 2 + 8;
	xy.y = src.size().height / 2;

	int w = dst.size().width;
	int h = dst.size().height;

	int srcStep = src.step;
	uchar* srcData = src.data;
	float p00, p01, p10, p11, pd;

	for (int j = 0; j < h; j++) {
		uchar* proLine = dst.data + dst.step * j;
		for (int i = 0; i < w; i++) {
			Point2f srcPoint = getCircleMapSrcPoint(i, j, xy.x, xy.y, r);
			float x = srcPoint.x;
			float y = srcPoint.y;
			int ix = (int) srcPoint.x;
			int iy = (int) srcPoint.y;

			// 4个点合成1个点
			p00 = srcData[iy * srcStep + ix * 3] * (1 + ix - x);
			p01 = srcData[iy * srcStep + ix * 3 + 3] * (x - ix);
			pd = (p00 + p01) * (1 + iy - y);
			p10 = srcData[(iy + 1) * srcStep + ix * 3] * (1 + ix - x);
			p11 = srcData[(iy + 1) * srcStep + ix * 3 + 3] * (x - ix);
			pd += (p10 + p11) * (y - iy);
			proLine[i * 3 + 0] = pd; // 目标点数据R

			p00 = srcData[iy * srcStep + ix * 3 + 1] * (1 + ix - x);
			p01 = srcData[iy * srcStep + ix * 3 + 3 + 1] * (x - ix);
			pd = (p00 + p01) * (1 + iy - y);
			p10 = srcData[(iy + 1) * srcStep + ix * 3 + 1] * (1 + ix - x);
			p11 = srcData[(iy + 1) * srcStep + ix * 3 + 3 + 1] * (x - ix);
			pd += (p10 + p11) * (y - iy);
			proLine[i * 3 + 1] = pd; // 目标点数据G

			p00 = srcData[iy * srcStep + ix * 3 + 2] * (1 + ix - x);
			p01 = srcData[iy * srcStep + ix * 3 + 3 + 2] * (x - ix);
			pd = (p00 + p01) * (1 + iy - y);
			p10 = srcData[(iy + 1) * srcStep + ix * 3 + 2] * (1 + ix - x);
			p11 = srcData[(iy + 1) * srcStep + ix * 3 + 3 + 2] * (x - ix);
			pd += (p10 + p11) * (y - iy);
			proLine[i * 3 + 2] = pd; // 目标点数据B
		}
	}
}

float MvPub::getLinearInsert(float calCurv[], float x)
{
	int Px = x;

	return calCurv[Px]*(1-(x-Px)) + calCurv[Px+1]*(x-Px);
}

float MvPub::getPointR(Point2f xy, Point2f dstPoint)
{
	return sqrt((dstPoint.x-xy.x)*(dstPoint.x-xy.x) + (dstPoint.y-xy.y)*(dstPoint.y-xy.y));
}


void MvPub::getCircleMapSrcPic(Mat &dst, Mat &src, float calCurv[], int calCurvNum)
{
	Point2f xy(0.f, 0.f);
	xy.x = src.size().width / 2;
	xy.y = src.size().height / 2;

	int w = dst.size().width;
	int h = dst.size().height;

	int srcStep = src.step;
	uchar* srcData = src.data;
	float p00, p01, p10, p11, pd;

	for (int j = 0; j < h; j++)
	{
		uchar* proLine = dst.data + dst.step * j;
		for (int i = 0; i < w; i++)
		{
			Point2f dstPoint(i, j);
			float dstR = getPointR(xy, dstPoint);

			int dstRR = (dstR);
			if (dstRR >= calCurvNum-1)
				continue;

			float calCurvVal = getLinearInsert(calCurv, dstR);

			float srtR = dstR*calCurvVal;

			float x;
			float y;
			if (dstR == 0)
			{
				x=0;
				y=0;
			}
			else
			{
				if (i<xy.x)
					x = xy.x - srtR*abs(xy.x-i)/dstR;
				else
					x = xy.x + srtR*abs(xy.x-i)/dstR;

				if (j < xy.y)
					y = xy.y - srtR * abs(xy.y - j) / dstR;
				else
					y = xy.y + srtR * abs(xy.y - j) / dstR;
			}

			int ix = x;
			int iy = y;

			if (ix>=720 || iy>=576)
			{
				continue;
			}
			if (ix<0 || iy<0)
			{
				continue;
			}

			// 4个点合成1个点
			p00 = srcData[iy * srcStep + ix * 3] * (1 + ix - x);
			p01 = srcData[iy * srcStep + ix * 3 + 3] * (x - ix);
			pd = (p00 + p01) * (1 + iy - y);
			p10 = srcData[(iy + 1) * srcStep + ix * 3] * (1 + ix - x);
			p11 = srcData[(iy + 1) * srcStep + ix * 3 + 3] * (x - ix);
			pd += (p10 + p11) * (y - iy);
			proLine[i * 3 + 0] = pd; // 目标点数据R

			p00 = srcData[iy * srcStep + ix * 3 + 1] * (1 + ix - x);
			p01 = srcData[iy * srcStep + ix * 3 + 3 + 1] * (x - ix);
			pd = (p00 + p01) * (1 + iy - y);
			p10 = srcData[(iy + 1) * srcStep + ix * 3 + 1] * (1 + ix - x);
			p11 = srcData[(iy + 1) * srcStep + ix * 3 + 3 + 1] * (x - ix);
			pd += (p10 + p11) * (y - iy);
			proLine[i * 3 + 1] = pd; // 目标点数据G

			p00 = srcData[iy * srcStep + ix * 3 + 2] * (1 + ix - x);
			p01 = srcData[iy * srcStep + ix * 3 + 3 + 2] * (x - ix);
			pd = (p00 + p01) * (1 + iy - y);
			p10 = srcData[(iy + 1) * srcStep + ix * 3 + 2] * (1 + ix - x);
			p11 = srcData[(iy + 1) * srcStep + ix * 3 + 3 + 2] * (x - ix);
			pd += (p10 + p11) * (y - iy);
			proLine[i * 3 + 2] = pd; // 目标点数据B
		}
	}
}
/*
float MvPub::bilinearInsert(Size size, srcdata, dstdata, )
{

}*/

